#include "../ComputeEdgeStrength.h"

#include <teem\ell.h>


float ApproximateFTLE(CPoint3F *pos, const float delta)
{
	float d = 2.0f * delta;

	float matJ[3][3];
	matJ[0][0] = ((pos[0].m_x - pos[1].m_x) + (pos[2].m_x - pos[4].m_x) + (pos[5].m_x - pos[7].m_x)) / (3.0f * d);
	matJ[1][0] = ((pos[0].m_y - pos[1].m_y) + (pos[2].m_y - pos[4].m_y) + (pos[5].m_y - pos[7].m_y)) / (3.0f * d);
	matJ[2][0] = ((pos[0].m_z - pos[1].m_z) + (pos[2].m_z - pos[4].m_z) + (pos[5].m_z - pos[7].m_z)) / (3.0f * d);
	matJ[0][1] = ((pos[3].m_x - pos[6].m_x) + (pos[2].m_x - pos[5].m_x) + (pos[4].m_x - pos[7].m_x)) / (3.0f * d);
	matJ[1][1] = ((pos[3].m_y - pos[6].m_y) + (pos[2].m_y - pos[5].m_y) + (pos[4].m_y - pos[7].m_y)) / (3.0f * d);
	matJ[2][1] = ((pos[3].m_z - pos[6].m_z) + (pos[2].m_z - pos[5].m_z) + (pos[4].m_z - pos[7].m_z)) / (3.0f * d);
	matJ[0][2] = ((pos[8].m_x - pos[17].m_x) + (pos[9].m_x - pos[18].m_x) + (pos[10].m_x - pos[19].m_x)) / (3.0f * d);
	matJ[1][2] = ((pos[8].m_y - pos[17].m_y) + (pos[9].m_y - pos[18].m_y) + (pos[10].m_y - pos[19].m_y)) / (3.0f * d);
	matJ[2][2] = ((pos[8].m_z - pos[17].m_z) + (pos[9].m_z - pos[18].m_z) + (pos[10].m_z - pos[19].m_z)) / (3.0f * d);

	float matJT[3][3];
	matJT[0][0] = matJ[0][0];
	matJT[1][0] = matJ[0][1];
	matJT[2][0] = matJ[0][2];
	matJT[0][1] = matJ[1][0];
	matJT[1][1] = matJ[1][1];
	matJT[2][1] = matJ[1][2];
	matJT[0][2] = matJ[2][0];
	matJT[1][2] = matJ[2][1];
	matJT[2][2] = matJ[2][2];

	double matA[9];
	matA[0] = matJT[0][0] * matJ[0][0] + matJT[0][1] * matJ[1][0] + matJT[0][2] * matJ[2][0];
	matA[1] = matJT[0][0] * matJ[0][1] + matJT[0][1] * matJ[1][1] + matJT[0][2] * matJ[2][1];
	matA[2] = matJT[0][0] * matJ[0][2] + matJT[0][1] * matJ[1][2] + matJT[0][2] * matJ[2][2];

	matA[3] = matJT[1][0] * matJ[0][0] + matJT[1][1] * matJ[1][0] + matJT[1][2] * matJ[2][0];
	matA[4] = matJT[1][0] * matJ[0][1] + matJT[1][1] * matJ[1][1] + matJT[1][2] * matJ[2][1];
	matA[5] = matJT[1][0] * matJ[0][2] + matJT[1][1] * matJ[1][2] + matJT[1][2] * matJ[2][2];
	
	matA[6] = matJT[2][0] * matJ[0][0] + matJT[2][1] * matJ[1][0] + matJT[2][2] * matJ[2][0];
	matA[7] = matJT[2][0] * matJ[0][1] + matJT[2][1] * matJ[1][1] + matJT[2][2] * matJ[2][1];
	matA[8] = matJT[2][0] * matJ[0][2] + matJT[2][1] * matJ[1][2] + matJT[2][2] * matJ[2][2];

	double eigenValues[3];
	ell_3m_eigenvalues_d(eigenValues, matA, AIR_TRUE);
	double ftle = sqrt(eigenValues[0]);
	return (float)ftle;
}



float EdgeStrengthMethodFTLE(CFiber **fibers, const int pos, const float delta)
{
	CPoint3F forwardPos[26];
	CPoint3F backwardPos[26];

	CVector3F dir = fibers[0]->m_seed.dir;

	for (int i = 0; i < 26; ++i) {
		CVector3F temp_dir = fibers[i+1]->m_seed.dir;
		if (InnerProduct(dir, temp_dir) > 0.0f ) {
			int index = (fibers[i+1]->m_fCount-1) > pos ? pos : (fibers[i+1]->m_fCount-1);
			forwardPos[i] = fibers[i+1]->m_pF[index];

			index = (fibers[i+1]->m_bCount-1) > pos ? pos : (fibers[i+1]->m_bCount-1);
			backwardPos[i] = fibers[i+1]->m_pB[index];
		} else {
			int index = (fibers[i+1]->m_fCount-1) > pos ? pos : (fibers[i+1]->m_fCount-1);
			backwardPos[i] = fibers[i+1]->m_pF[index];

			index = (fibers[i+1]->m_bCount-1) > pos ? pos : (fibers[i+1]->m_bCount-1);
			forwardPos[i] = fibers[i+1]->m_pB[index];
		}

	}

	float forwardFTLE = ApproximateFTLE(forwardPos, delta);
	float backwardFTLE = ApproximateFTLE(backwardPos, delta);
	
	float ftle = forwardFTLE > backwardFTLE ? forwardFTLE : backwardFTLE;
	return ftle;
}